#include <stdio.h>
#include <avr/io.h>
#include "can_avr.h"

void clearMobRegisters( void )
{
	uint8_t  volatile *__i_; 
	for (__i_=&CANSTMOB; __i_<&CANSTML; __i_++) {
		*__i_=0x00 ;
	}
}

void CAN_init( void )
{
	uint8_t mob_number;
	
	//Setup CAN
    CANGCON = (1<<SWRES);
    CANBT1 = 0x06; 				// Tscl = 4x Tclkio = 250 ns
	CANBT2 = 0x04;		        // Tsync = 1x Tscl, Tprs = 3x Tscl, Tsjw = 1x Tscl
	CANBT3 = 0x13;       		// Tpsh1 = 2x Tscl, Tpsh2 = 2x Tscl, 3 sample points

    for( mob_number = 0; mob_number < NB_MOB; mob_number++ ) {
        CANPAGE = (mob_number << 4);    //! Page index
		clearMobRegisters();
    }
    CANGCON |=  (1<<ENASTB);	//enable CAN
}


void CAN_objTxOnce( mobCtrl_t *mob )
{
	uint32_t u32tmp;
	uint8_t i;
	
	CANPAGE = (mob->mob << 4);
	
	clearMobRegisters();
	
	u32tmp = mob->msgId;
	CANIDT1   = CAN_SET_STD_ID_10_4(u32tmp);
	CANIDT2   = CAN_SET_STD_ID_3_0( u32tmp);
	CANCDMOB &= (~(1<<IDE));	//make sure IDE flag is clear
	CANCDMOB |= mob->dlc;
	
	mob->cmd = MOB_Tx;

	for( i = 0; i < mob->dlc; i++ ) {
		CANMSG = mob->data[i];
	}
	CANCDMOB |= (MOB_Tx_ENA << CONMOB0);
}


void CAN_objRx( mobCtrl_t *mob, uint8_t mobNumber, uint16_t msgId, uint8_t dlc )
{
	uint32_t u32tmp;

	mob->mob = mobNumber;
	mob->msgId = msgId;
	mob->dlc = dlc;
	mob->cmd = MOB_Rx;
	CANPAGE = (mob->mob << 4);
	
	clearMobRegisters();
	
	u32tmp = mob->msgId;
	CANIDT1   = CAN_SET_STD_ID_10_4(u32tmp);
	CANIDT2   = CAN_SET_STD_ID_3_0( u32tmp);
	CANCDMOB &= (~(1<<IDE));	//make sure IDE flag is clear
	CANCDMOB |= mob->dlc;
	
	u32tmp = ~0;
	CANIDM1   = CAN_SET_STD_ID_10_4(u32tmp);
	CANIDM2   = CAN_SET_STD_ID_3_0(u32tmp);
	CANIDM4 &= ~(1<<RTRMSK);
	CANIDM4 |= (1<<IDEMSK);

	CANCDMOB |= (MOB_Rx_ENA << CONMOB0);	//setup for Rx
}


mobStatus_e CAN_objStatus( mobCtrl_t *mob )
{
	uint8_t i;
	
	CANPAGE = (mob->mob << 4);			//select page
	if( (CANSTMOB & (1<<RXOK)) && (mob->cmd == MOB_Rx) ) {
		//data has been recieved - read it out into data[]
		for( i = 0; i < mob->dlc; i++ ) {
			mob->data[i] = CANMSG;			//auto-increments through the MOb's buffer
		}
		CANSTMOB = 0;
		CANCDMOB |= (MOB_Rx_ENA << CONMOB0);
		return MOB_Complete;
	}
	else if( (CANSTMOB & (1<<TXOK)) && (mob->cmd == MOB_Tx) ) {
		//data has been transmitted ok
		CANCDMOB &= ~(1<<CONMOB0 | 1<<CONMOB1);	//disable the MOB
		CANSTMOB = 0;							//clear status too
		//thats it.  We need to re-issue the command to re-transmit the data
		//we had to call this objStatus to increment the tx counter
		return MOB_Complete;
	}
	else if( CANSTMOB & ((1<<BERR) | (1<<SERR) | (1<<CERR) | (1<<FERR) | (1<<AERR)) ) {
		return MOB_Error;
	}
	else {
		return MOB_Busy;
	}
}

void CAN_objAbort( mobCtrl_t *mob )
{
	CANPAGE = (mob->mob << 4);			//select page
	CANCDMOB &= ~(CONMOB0 | CONMOB1);	//disable the MOB
	CANSTMOB = 0;						//clear status too
}
